#!/bin/bash
PATH=/bin:/sbin:/usr/bin:/usr/sbin:/usr/local/bin:/usr/local/sbin:~/bin
export PATH
GOLO_Ver='0.3'
. /opt/openresty/nginx/scripts/golo.conf
source $ROS_PATH
source $ROS_GOLO_WS_PATH
export ROS_IP=$ROSIP
ecuswith=`rosparam get /ecu_swith`
rosservice call /move_base/clear_costmaps
rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$tebcar &
rosnode kill /scan_multi_merged
rosnode kill /null_scan_relay
roslaunch merged_percept scan_multi_merged.launch &
sleep 3.5
init_times=1
while true
do
    rosSM=$(rosnode list|grep scan_multi_merged |wc -l)
    if [ $init_times -ge 25 ];then
	break
    fi
    ecucost_data=$(rosparam get /move_base/TebLocalPlannerROS/penalty_epsilon |grep 0.04 |wc -l)
    ecucost_car=$(rosparam get /move_base/base_global_planner |grep global_planner |wc -l)
    if [ $ecucost_car -ne 1 ];then
   rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$tebcar &
    fi
    if [ $ecucost_car -eq 1 ] && [ $ecucost_data -ne 1 ];then
	 rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$ecucostcar &
    fi
    if [ $ecucost_data -eq 1 ] && [ $rosSM == 1 ];then
       rosparam set /ecu_swith false
       break
    fi
    init_times=$(($init_times+1))
    ecucost_data=$(rosparam get /move_base/TebLocalPlannerROS/penalty_epsilon |grep 0.04 |wc -l)
    rosSM=$(rosnode list|grep scan_multi_merged |wc -l)
    if [ $ecucost_data -eq 1 ] && [ $rosSM == 1 ] && [ $init_times -eq 3 ];then
	    sleep 0.1
	    break
    fi
    sleep 0.2
done




